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PULSE  DOPPLER  AMBIGUITY  RESOLUTION 


The  author  of  this  note  has  been  involved  with  the  analysis  of 
existing  and  future  pulse  doppler  ambiguity  resolution  methods.  The 
existing  pulse  doppler  ambiguity  resolution  technique  is  supplied  by 
R.C.A.  and  a future  algorithm  is  presented  by  this  author. 


P..C  .A. 

The  R.C.A.  algorithm  is  a least  squares  range  error  minimisation. 
Given,  the  state  differential  model  of  range  and  range-rate  informa- 
tion 

x(t)  « Ax  + Br  + w , x(0)  ■ xQ  (1) 

where  (°)  de:  otes  d/dt 

x(t)  - Ix^ t)\  , w(t)  - / w^ (t) \ 

\x2(t)/ 

x^(t)  - range  yds 

X2(t)  ■ range-rate  yds/st" 

r(t)  ■ observed  range  yds 

r(t)  ■ observed  range-rate  yds/sec 

w^(t)  ■ range  measurement  error 

W£(t)  ■ range-rate  measurement  error 


where 


A - 

and  6 


■ yds/sec/  spectral  line  conversion  factor. 


(2) 


Let  the  system  observations  be  given  by 


y - Cx  + Wj(t)  (3) 

where  C « (10)  (4) 

end  w^(t)  - observation  measurement  error. 


R.C.A.  uses  the  following  criterion  for  optimality.  They  choose 
to  minimize  J,  where 


J - 


fT 

II  r(t)  - x1(t)||2  dt  - 

• 0 


fT 

*•0 


| | e^^Ct)  | | 2 dt 


(5) 


The  conditions  for  optimflity  are  well  known  and  are  given  by  the 

maximum  principle  of  Pontryagin.  It  is  interesting  to  note  that  R.C.A 

chooses  to  minimize  range  error  only.  They  do  not  attempt  to  minimize 

2 

the  range-rate  error  e2<t).  Secondly,  the  normed  error  ||e^(t)||  is 
implicitly  weighted  by  1 "one".  This  may  not  seem  critical,  but  it 
means  that  all  errors  are  considered  equally  independent  of  system 
signal  to  noise  ratios.  Heuristically,  if  the  range  measurement 
errors  are  stochastically  small,  then  any  difference  between  r(t)  and 
its  estimate  x^(t)  is  significant.  However,  if  the  range  measurement 
noise  is  stochastically  large,  then  apparent  differences  between  r(t) 
and  x^(t)  are  no  longer  significant.  In  this  case,  only  long  term 
error  trends  should  be  considered  significant. 

In  summary,  the  author  feelB  that  there  are  some  philosophical 
disadvantages  associated  with  R.C.A.  methods.  They  totally  iguore 
range-rate  errors  in  their  optimality  criterion  and  they  do  not 
weight  the  apparent  error  r(t)  - x. (t)  by  a measure  of  system 


signal  to  noiss.  These  two  features  make  their  method  especially 
sensitive  to  scintillations.  This  will  be  demonstrated  in  a simula- 
tion found  at  the  end  of  this  report. 

The  computational  method  used  to  solve  the  required  necessary 
linear  differential  equations  which  define  the  optimal  solution  is 
the  method  of  invariant  imbedding.  This  method  is  generally  used  on 
nonlinear  mixed  two-point  boundary  value  problems.  Needless  to  say, 
using  the  invariant  Imbedding  method  is  a tremendous  overkill  when 
applied  to  their  two  dimensional  mixed  two-point  boundary  value 
differential  equation. 

R.C.A.  also  uses  an  unusual  technique  to  measure  the  accuracy  of 
their  algorithm.  They  compute  the  variance  of  the  apparent  error 
r(t)  - x^t)  (This  is  called  traditionally  an  innovations  process). 

If  the  error  implies  an  error  of  greater  than  1/2  spectral  line 
the  estimation  of  r(t),  namely  x2(t)  is  considered  to  be  bogus  and 
the  estimation  process  is  aborted.  This  is  meaningful  under  ideal 
low  noise  conditions  only.  As  mentioned  before,  R.C.A.  does  not  use 
Information  about  system  measurement  noise  (signal  to  noise) . 
Therefore,  i,'.  a low  noise  case  the  variance  of  the  error  r(t)  - x^(t) 
does  indeed  represent  estimation  error  based  on  the  R.C.A. 's  estima- 
tion algorithm.  However,  If  the  system  measurement  noise  it 
stochastically  large,  the  estimations  of  r(t),  namely  x2(t),  could  be 
statistically  good  but  the  error  variance  measure  of  r(t)  - x^(t) 
could  be  large  due  to  system  noise  and  not  estimation  errors.  This 
phenomenon  has  caused  the  system  to  "flag,"  an  error  in  the  r(t) 
estimation  when  indeed  there  was  no  such  error. 


In  1972  the  author  of  this  report  developed  a new  algorithm 
using  invariant  imbedding  techniques.  The  objective  function 
to  be  minimised  was  J,  where 


J - 


r(t)  - *1<t)llyi(t)  + ! I ?<t>  - x2(t)  - fixx(t)| |J  ^ 


dt 


I el^t)ilwi(t)  + II  e2^Hw2(t)  dt 


(6) 


Here,  both  range  and  range-rate  errors  are  minimized.  In  addition, 
these  errors  are  weighted  by  W^(t)  and  W2(t),  respectively.  The 
weight  W^(t)  and  W 2 (t)  are  chosen  to  be  the  inverse  of  the  assumed 
a priori  error  variances  associated  with  e^(t)  and  e2(t).  The 
advantages  of  this  technique  can  be  found  in  a simulation  found  in 
the  paper  included  in  appendix  A. 

A new  algorithm  has  been  developed  which  holds  promise  as  an  r 
dot  extraction  system.  It  has  been  found  to  be  the  most  accurate 
and  versatile  algorlti.?  tested  to  date.  Its  origin  is  rooted  in 
the  theory  of  minimal  variance  filter  (Kalman  filter). 

Briefly,  the  minimal  variance  filter  is  known  to  be  "the" 
optimal  estimation  algorithm  for  linear  systems  being  corrupted  by 
white  noise  with  known  covariances.  The  Kalman  filter  has  been 
successfully  applied  to  a myriad  of  linear  system  problems.  The 
classic  difficulties  associated  with  Kalman  filters  will  be  noted  at 
the  end  of  this  section.  The  derivation  of  the  algorithm  is  derived 


'i 


*L. 


as  follows: 

given  equation  (1),  nauely  for  x(t):  n dimensional 
x(t)  " Ax(t)  + B(t)?(t)  + w(t) 
with  x(0)  given  by 

x1(0)  - r(0) 

*2(°)  “ 0 

and  equation  3 for  y(t)  a scalar 
Y(t)  - Cx(t)  + v*t) 

Let  the  a priori  noise  statistics  be  given  by 
£ (w(t) ) « 4 
£ (w3(t))  - $ 

cov  (w(t)  wT(t))  • V (t)  fi(t  - t) 

w 

cov  (x(0))  - Vx(0) 

T 

COV  (w(t)  W3  (t))  ■ 0 

cov  (w3(t)  w3T(t))  - Vv6(t  - x) 

Define  the  estimation  error  to  be 
x(t)  - x(t)  - a(t) 

There  exists  a n x n dimensional  positive  definite  symmetric  matrix 
error  covariance  matrix,  say  V^(t),  such  that 

cov  (*(t)  *T(t))“  cov  ((x(t)  - *(t))(x(t)  - ftT(t)  ■ V^t) 


(10) 


and 


?a(t)  - AVa(t)  + V^Ct)  AT  - Va(t)  CTVy“l(t>  CVx(t)  + Vy(t) 


with 


va(0)  - vx(0) 


With  respect  to  the  state  ambiguity  problem,  V^(t)  becomes,  for  n 


(ID 

2 


■ 2s\Mn  + \ - vS2(t)n  V1 


^5t(t)12  “ V°22  6 " V*(t)io  v»(‘)ii  V.(t)’1 


*'fc,12  ’*Vfc/ll  v 


- Vwj<‘>  - ViW12  V1 


with  vs(0)n  - »x(«)u  . Vx(0)22  - Vx<0)22  . Vs(0)12  - yo>12  - 0 (13) 


The  general  estimation  of  x(t)  in  the  presence  of  noise  w(t)  and  v(t) 


is  given  by,  say  £(t) 


S(t)  - A£(t)  + Br (t)  + K(t) [y (t)  - C*(t)] 

where  X(0)  - ^ (x(0)) 

where  denotes  expected  value 


(14) 

(15) 


where  K(t)  is  referred  to  as  the  Kalman  gain  and  satisfies 


K(t)  - V^(t)  cT  vv_1(t) 


For  the  problem  under  consideration 


In  general,  K(t)  Is  precomputed  and  stored  off-line  (aee  appendix  F). 

The  resulting  estimation  differential  equation  la 

o o 

lx(t)  - 6x2(t)  + R^t)  (r(t)  - ^(t))  + r(t)  (17) 

o 

*2(t)  - K2(t)  (r (t)  - ^(t)) 

This  algorithm  has  been  under  teat  and  has  performed  extremely  well. 
The  results  of  this  test  are  compared  side-by-side  with  the  existing 
R.C.A.  algorithm.  It  always  produced  a superior  answer  to  that 
obtained  using  R.C.A. 's  method.  In  many  cases  the  Improvement  was 
drauiatic.  The  results  of  the  simulations  can  be  found  in  appendix  B. 

As  previously  noted  there  are  some  potential  difficulties 
associated  with  Kalman  filtering.  They  are  in  defining  (usually 
assuming)  the  initial  covariances  Vw(t),  Vy  (t),  and  vx(0).  These 
quantities  are  generally  assumed  to  be  known.  However,  if  the 
assumption  differs  considerably  from  the  actual  noise  covariancea, 
poor  estimation  and  even  divergence  can  result.  Therefore,  it  is 
important  that  a reasonably  accurate  estimate  of  these  noise 
covariances  be  made  if  satisfactory  performance  is  to  be  insured. 

The  author  of  this  report  has  several  suggestions  which,  when 
implemented,  should  provide  W.S.M.R.  with  a very  powerful  and 
sophisticated  r dot  estimation  system. 

Suggestion  1 

The  actual  error  covariance  process,  say  V^(t) 
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! i 

\ ; 


s - 
[ * 


VO  - E 


(r0(t)  - Jjd))2  (t0(O  - Vt))<f0(t>  - *2<‘) 
(Sfl(t)  - *,(c)(r0(t)  - tjU)  - *2(t))2 


can  ba  computed  directly  uaing  observed  data  and  the  estimetion  vector 
*(t).  The  actual  covarianca  V^Ct)  can  be  compared  with  the  theoret- 
ical error  covariance  V^(t) • The  resultant  error  will  then  be  used 
to  repro grant  the  a priori  assumption  on  V^Ct)  and  V^Ct). 


Suggestion  2 

The  MPS-36  offers  a unique  feature  which  can  serve  to  rescale 
the  a priori  covarianca  nwtrlces.  Ihat  feature  is  the  system  A.G.C. 

The  output  of  the  A.G.C.  is  a measure  of  system  slgnal-to*nolae  ration. 
This  real-time  measure  of  noise  can  be  used  to  rescale  the  covariance 
data  and  thereby  optimise  the  algorithm.  Typical  A.G.C.  noise  measure 
data  is  presented  and  discussed  in  appendix  C.  A new  algorithm,  vhicn 
shall  be  called  an  adaptive  Kalman/ A.G.C.  algorithm  shall  now  be 
developed. 


Adartive  Filtering 

It  was  assumed  throughout  this  work  that  over  an  averaging  inter- 
val, say  5 seconds,  the  noise  statistics  are  stationary.  A filter 
whose  a priori  statistics  are  assumed  to  ba  constant  over  an  averaging 
interval  shall  be  called  a constant  covariance  filter.  The  noise 
covariances  associated  with  the  processes  x^  and  and  or  equiv- 
alently the  output  process  y should  be  scaled  proportional  to  the 
api*  rant  A.G.C.  slgrel  to  noise  ratio.  It  shall  be  assumed  that  over 


i 

i 


j 
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an  averaging  interval,  the  estimation  subsystem'.:,  noise  covariance 
possesses  the  asms  qualitative  time  varying  properties  of  the  A.G.C. 
noise  meteric.  Consider  the  following  simulated  result  which  exem- 
plifies the  properties  of  the  adaptive  Kalman/A. G.C.  approach  to  pulse* 
doppler  parameter  ectiraation. 


Example:  Kalman  Filter 

The  developed  Kalman  Filter  fine  line  estimation  algorithm  was 
implemented  cn  r POP-11/45  at  The  University  of  Texas  at  El  Paso..  A 
source  listing  can  be  found  in  appendix  D.  Several  numerical  exper- 
iments were  performed  on  the  algorithm.  All  tests  Involve  5 second 

averaging  Intervals,  and  an  initial  target  rango,  velocity,  and 

o 

acceleration  of  100,000  yds,  -1000  yds/sec,  and  20  yds/sec  respectively. 


The  first  test  investigates  the  algorithms  sensitivity  to  the 

choice  of  input  covariances  V and  V for  a given  slgnal/nolse  ratio. 

Wi  W2 

The  experimental  results  can  be  found  in  figure  K 1.  It  can  be  noted 

that  of  the  parameteric  values  tested,  all  choices  tested  successfully 

in  that  the  error  at  5 sec  was  considerably  less  than  +.5  spectral 

lines.  However,  qualitatively  there  were  differences.  It  can  be  seen 

that  as  the  a priori  estimate  of  V , and  V decreases,  the  estimation 
r w’  w^ 


input  noise  decreases  (ex:  V ® .001.  V 

tl  w 

1 w2 


time  constants  also  increase.  That  is,  as  the  a priori  assumption  on 

.001),  the  Kalman  filter 
is  very  reluctant  to  change  its  previous  estimate  since  it  is  assumed 
tliat  the  estimate  was  obtained  in  a good  signal/ to  noise  environment. 
Contraposltively,  If  the  input  noise  covariances  are  assumed  to  be 
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large  (ex:  V * .1,  V ■ .1)  the  Kalman  filter  will  readily  change 

W1  w2 

its  previous  estimate  since  it  consider  the  previous  estimate  sta- 
tistically uncertain  due  to  increased  signal/noise  ratio.  To  use 
the  Kalman  filter  optimally,  the  covariance  weights  should  be  deter- 
mined experimentally  or  through  simulation.  A trade  off  is  sought 
between  fast  response  ana  therefore  possible  inaccuracies  and  a slew 
response  which  may  present  incomplete  estimate  at  the  end  of  an 
estimation  period. 

l 

The  second  experiment  found  in  figure  K 2,  considers  a parame- 
terization of  V for  the  indicated  signal  to  noise  ratio.  Herr,  v 

W1  W1 
is  the  noise  corrupting  the  range  constraint  equation  x^(t)  (see  eq.  1) . 

Due  to  the  advantageous  large  signal/noise  ratio,  all  parameterizations 
were  successfull  and  performed  essentially  the  same.  Therefore,  it  can 
be  assumed  that  the  algorithm  will  work  well  over  a wide  range  of 
assumptions  in  the  presence  of  good  range  data. 


The  third  experiment,  found  in  figure  K 3,  considers  a parame- 
terization of  V for  the  indicated  signal/noise  ratio.  It  can  be 
w2 

noted  that  the  algorithm  is  sensitive  to  the  choice  of  V when  V - 

w2  w2 

1,  the  Kalman  filter  error  gains  are  large.  Therefore,  the  estimate 

reacts  rapidly  to  correct  any  apparent  error.  When  V ■ .01,  the 

w2 

Kalman  filter  error  gains  are  small.  This  means  the  algorithm  can 
not  react  rapidly  to  apparent  error.  This  explains  the  large  error 
between  .2  and  .6  seconds.  Here  it  took  approximately  .8  seconds  to 
purge  the  Initial  estimation  error  from  the  estimation  processes. 
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1 is 


Convergence  is  slow  but  methodical.  The  performance  for  V 

w2 

found  to  be  a compromise  between  the  two  extremes. 

The  experiments  considered  use  a standard  Kalman  filter  approach 
to  the  fine  line  tracking  problem  in  the  presence  of  short  term 
stationary  noise.  The  following  experiments  consider  a radical  time 
varying  of  noise  behavior.  This  hypothesis  represents  simulated 
scintillation. 

The  fourth  experiment  found  in  figure  K-4  Involves  such  test. 

Here  a noise  burst,  over  the  interval  1 to  2.5  seconds,  was  numeri- 
cally generated  to  decrease  by  10  dB,  the  nominal  range  and  range- 
rate  signal  to  noise  ratio  (assumed  to>  be  20dB) . As  a reference 

experiment,  V ■ V ■ .01,  and  V “1,  for  all  time  was  assumed.  It 
W1  W2  V 

can  be  noted  that  during  periods  of  high  scintillation,  the  reference 
algorithm  behaved  radically.  The  errors  generated  during  this  period 
remained  with  the  estimation  process  in  the  future.  Recall  a decrease 
in  produces  a decrease  in  the  Kalman  error  gain.  Thus,  a A.  G.  C. 
noise  meterlc  feedback  can  be  used  to  reduce  the  Kalman  gain  during 
periods  of  high  scintillation.  This  reduced  gain  will  forbid  the 
system  to  rapidly  track  apparent  errors  which  are  known  to  come  from 
a noisy  environment.  It  can  be  noted  that  with  the  proper  scaling  of 
V^,  significant  improvements  in  the  estimation  processes  can  be  obtained. 

The  fifth  experiment  found  in  Figure  K 5 involves  such  a scintil- 
lation test.  Here  a noise  burst,  over  the  interval  1 to  2.5  seconds, 
was  numerically  generated  to  decrease,  by  Jn!B,  the  nominal  range  and 
range-rate  signal  to  noise  ratio  (assumed  to  be  20dB).  As  a reference 
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experiment,  V ■ V ■ .01  and  V * 1,  for  all  time  was  assumed.  It 
can  be  noted  that  during  period!  of  high  scintillation,  the  reference 
algorithms  behaved  radically.  The  error  and  error  rates  that  were 
resident  in  the  algorithm,  when  the  noise  figure  returned  to  a 
nominal  value  (le:  at  t - 2.5  seconds)  ware  such  that  the  future  error 
performance  was  poor.  It  shall  be  shown  that  the  Kalman  gains  are 
Inversely  proportional  to  V^.  (This  la  implicitly  the  same  as  reducing 
the  value  of  Vw« ) Therefore,  increasing  the  value  of  the  observation 
(output)  covariance  in  harmony  with  an  A.G.C.  noise  meteric,  to 
say  4 to  10,  will  selectively  reduce  the  error  gains  in  the  presence 
of  noise  burst.  This  has  the  property  of  stabilizing  the  estimate  to 
be  a minor  variation  in  the  last  estimate  generated  under  good  signal/ 
noise  ratio.  It  can  be  noted  that  significant  fine  line  estimation 
performance  can  be  expected  using  an  adaptive  Kalman/A. S.C.  philosophy. 

It  is  of  interest  to  more  closely  examine  the  quantitative 

properties  of  the  Kalman  gains  under  varied  parameteric  conditions. 

The  trajectories  of  the  Kalman  gains  K^(t)  and  KjCt)  (see  eq.  13  and 

16)  can  then  be  found  in  figures  Kl-1,  Kl-2,  K2-1,  and  K2-2.  Consider 

first  Kl-1.  It  can  be  noted  the  Kalman  gain  Kl(t)  is  inversely  related 

the  differences  between  V and  V . 

w v 

This  property  can  be  witnessed  again  in  figure  Kl-2.  A scin- 
tillation buret  is  assumed  to  occur  from  1 to  2.5  seconds.  Here, 
rescaling  will  cause  a decrease  in  the  error  feedback  gain  K 1 


during  periods  of  high  scintillation.  It  is  interesting  to  note  that 
the  trajectory  time  constants  are  small  compared  to  the  averaging 
interval.  This  feature  will  be  developed  in  the  next  section.  The 
arguments  associated  with  the  Kalman  ^(t)  are  identical  to  the 
used  with  the  Kalman  gain  K^(t). 

Steady-State  Kalman  Filter 

Since  it  can  be  noted  that  the  Kalman  gain  trajectories  tend  to 
their  steady-state  value  rapidly  (with  respect  to  the  averaging 
interval)  replacing  the  time  varying  Kalman  gains  with  their  steady 
state  value  would  appear  to  be  justifiable.  In  additirn,  if  the 
numerical  integration  of  equation  13  can  be  bypassed . Thus  a potential 
problem  as  numerical  divergence  of  the  generating  differential  equa- 
tion when  large  value  of  V or  V are  used,  is  avoided. 

w v 

The  steady  stat^  covariance  matrix  V^(t)  positive  definite 
symmetric  error  must  satisfy 


d V*(t) 
dt 


0 


(18) 


therefore 


0 


2 6 V. 


+ V 


12 


w. 


0 - 


0 


(19) 
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which  simplifies  to  the  following  algebraic  relationship 


(V 


w V 
2 


1/2 


(V  (2  6 V-  + V ))1/2 
v x12  Wj. 


V 6 
v 


(20) 


a source  listing  of  a steady  state  Kalman  gain  algorithm  can  be 
found  in  appendix  E.  There  are  several  distinct  advantages  associated 
with  a steady  state  algorithm.  First,  it  1b  trivial  to  Implement 
since  it  only  requires  the  algebraic  computation  of  the  gains  found 
in  equation  (20) . These  gains  are  formally  substituted  into  the 
estimation  differential  equation  (14) . This  equation  is  solved  numer- 
ically using  any  method  applicable  to  constant  coefficient  state 
differential  systems  (see  appendix  G) . Secondly,  a problem  common  to 
all  numerical  Integration  methods  is  that  they  may  diverge  (ie: 
induce  a floating  point  error)  for  certain  parameterlc  events.  This 
is  particularly  true  when  dealing  with  the  error  covariance  differ- 
ential equation  found  in  Kalman  filtering.  The  algebraic  eq’iation 
found  in  equation  (20)  will  produce  an  approximation  error  covariance 
which  cannot  become  numerically  unstable.  A numerical  experiment 
used  to  demonstrate  the  effects  of  an  adaptive  Kalman/A. G.C.  philosophy 
is  treated  using  the  steady-state  algorithm.  Again,  the  a priori 
noise  covariance  shall  be  chosen  to  be  in  harmony  with  the  A. G.C. 
error  meterlc.  The  result  of  this  experiment  can  be  found  in  figure  KSSvl 


*4 


-2.  The  qualitative  results  obtained  are  similar  to  those  obtained 
using  an  adaptive  Kalman/A. G.C.  algorithm.  Due  to  it  being  purely 
algebraic,  cannot  diverge  if  its  parameters  are  finite. 

Tc  show  the  utility  of  such  a technique  numerical  experiments 
were  performed.  The  initial  target  information  was  identical  to 
that  found  in  the  Kalman  filter  test.  However,  the  time  varying 
Kalman  gains  shall  now  be  replaced  by  a steady-state  approximation. 

The  first  experiment,  described  in  figure  KSS-1,  shows  that  the 

steady-state  algorithm  error  estimate  does  converge  to  an  acceptable 

error  value  over  a wide  range  of  V . The  adaptive  A. G.C.  philosophy 

was  also  integrated  in  a steady-state  Kalman  filter  configuration. 

The  results  of  this  experiment  can  be  found  In  figure  KSS-2.  A 

reference  test  using  a lOdB  decrease  of  a nominal  signal  to  noise 

ratio,  namely  30dB,  was  assigned  the  parameteric  value  of  V ■ V ■ 

W1  w2 

.1  and  V - 1.  It  can  be  seen  that  erratic  estimation  behavior  occurs 
v 

in  the  presence  of  strong  scint-« llation.  As  the  a priori  output 
covariance  parameter  is  increased  in  harmony  with  increasing  noise, 
the  steady-state  Kalman  error  gain  decreases.  The  reduction  in  gain 
results  in  reduced  fluxuatlons  in  the  fine  line  estimate  during  a high 
noise  condition.  The  adaptive  steady-state  Kalman  gain  algorithm  is 
an  improved  estimate. 
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Conclusions 


The  existing  R.C.A.  ambiguity  resolution  method  has  been  tested 
and  found  to  perform  satisfactorily  when  the  received  data  is  scin- 
tillation free.  These  noise  bursts  produce  extremely  large  estimation 
errors  and  poor  convergence  properties,  this  is  due  to  their  algorithm 
minimizing  only  range  error,  with  a fixed  time- Invariant  weight  (name- 
ly the  number  1) . 

The  new  algorithm  using  Kalman  filter  produces  superior  results. 
It  embodies  the  simultaneous  minimization  of  both  range  and  range- 
rate  errors  and  uses  optimal  time  varying  weights,  namely  covariance 
information.  It  is  forcefully  felt  that  using  Suggestion  2 as  the 
modifier,  the  Kalman  filter  algorithm  would  give  W.S.M.R.  a reliable 
and  flexible  MPS-36  based  r dot  extraction  system.  In  this  config- 
uration, system  performance  would  be  limited  primarily  by  hardware 
limitations  Intrinsic  to  the  system. 
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Pulse  Doppler  Ambiguity 
Resolution 

FRED  J.  TAYLOR 

The  University  of  Texas  at  El  Paso 

El  Paso.  Tex.  79968 


Abstract 

A mathematical  algorithm  using  the  method  of  invariant  Imbedding 
b developed  which  generates  e beat  2?  estimate  of  range  and  range 
rate  from  puiae  Doppler  date. 


A pulse  Doppler  system  has  the  ability  to  track  moving 
targets  in  relatively  stationary  clutter  in  the  pretence  of 
high  energy  noise.  The  spectral  representation  of  a pulse 
train  with  a given  PRF  and  pulsewidth  r is  given  by  the  tin 
(X)/X  function  whose  firs:  zero  crossings  occurs  at  1/r  and 
the  special  frequency  between  adjacent  spectral  lines  is 
1/PRF.  Thus  at  band  (S650  MHz  = /0)  a Af  - 1 Hz  implies 
a resolution  of  0.029  yd/s  and  a PRF  - 640  yields  a 
spectral  spacing  of  I8.S6  yd/s  between  lines.  If  a pulse* 
width  of  lps  is  considered,  then  there  exists  over  1500 
spectral  lines  in  the  interval  \f0,  /„  + 1/r],  Thus  a pulse 
Doppler  system  is  cursed  by  an  abundance  of  ambiguous 
spectral  data  about  any  arbitrary  spectral  line. 

One  ambiguity  resolution  technique  that  has  been 
implemented  uses  the  method  of  invariant  imbedding  ( 1 J . 
The  algorithm  developed  was  a direct  adaptation  of  a set  of 
notes  published  by  Bellman  and  Kalaba.  Herein,  the  optimal 
estimate  was  one  that  gave  the  “best”  £2  fit  to  the  observed 
range  data.  Since  range  rate  is  often  considered  to  be  a 
more  accurate  data  source  than  range  data,  any  optimal 
estimate  should  be  optimally  fitted  to  range-rate  data  also. 

Consider  then  the  following  problem  in  terms  of  the 
following  state  variables: 


xt  (t)  = actual  range 

x2  (r)  - number  of  spectral  lines  (real  number) 

X|  °(r)  — observed  range. 

Observation  dynamics: 

range  x,°(r)  = x,(r)-  e,(r), 

fi  (t)  = measurement  error  ( 1) 

range  rate  x,  °(t)  = x,  (t)  - x2  (f) 6 - e,  (f), 

e,(r)  = measurement  error  (2) 

where  5 = 18.56  if  PRF  - 640. 

If  an  estimate  x2  (number  of  spectral  lines  in  error  from 
a coarse  track  spectral  line)  is  assumed  to  be  constant  over 
an  observation  interval,  then  require 

*s=0.  (3) 

If  a time-varying  estimate  o(  x2(r)  is  desired,  then  x2(r) 
may  be  approximated  by  a power  series  in  t whose 
coefficients  are  chosen  optimally.  However,  the  additional 
dimensionality  requirements  imposed  on  the  solution  proc- 
ess makes  this  approach  unattractive  in  general. 

Defining 

^ = (j?i,x2Jr  (estimation  vector)  (4) 

£ = t^i . ) T (estimated  error  vector),  (5) 

Manuscript  received  February  28, 1972.  then 

Thii  work  wai  supper .cd  in  part  under  U.R. I.  Grant  083-50-790-04.  xi  s=*t°  (range  estimate)  (6) 
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Flfl.  1.  Syttim  modal. 


and  problem),  in  terms  of  2 (estimated  range  error): 

. i|°  + (range-rate  estimate)  * m ~ 1 ^i($i  -*1°)  + *»$  1 WtX  + *a^8 

.*'}  L 0 J (spectral  error  estimate).  £,=/>„*/,(£  -Xl°)=P2lWti,  2(0),  ^(0)  arbitrary 

(7)  (12) 

Let  it  be  required  that  an  objective  function  J be  and 


. =2/>J18+/’ll1H'1  - HWt  j Aa  =PJJ6-/>.i/,IJh', 

P= f----- 

Pit— Pti  j Pn=P\i7Wi 


m 

arbitrary. 


minimized: 
J = 


= T / ifll***. 

♦/o 


JV  > 0,  diagonal. 


The  necessary  conditions  for  producing  an  optimal  estimate 
x(r)  are  given  by  the  Maximum  Principle  [2],  and  they 
define  the  following  two-point  boundary  value  problem. 
State  equations: 


Equation  (13)  is  solved  off-line  and  stored  to  facilitate  a 
real-time  estimation  x (see  Fig.  1).  Since  all  the  initial 
conditions  are  arbitrary  it  is  assumed  that  they  would  be 
determined  experimentally  under  actual  mission  conditions. 
Finally,  it  is  known  from  linear  estimation  theory  that  the 
optimal  choice  of  W is  the  inverse  of  the  measurement  error 
covariance  matrix  which  should  be  used,  when  available,  to 
admit  a minimal  variance  estimate  ofx. 


x - f{x,  X>=  ^ 
Costate  equation: 
X = g{ x.  X)  = 


V'  +x,5-V^ 
0 


b',(*,°-x,) 

-M 


MO)  = 0 

X(7)  = 0. 


One  technique  used  to  solve  two-point  boundary  value 
problems  is  the  method  of  invariant  imbedding  (3] . The 
imbedding  equation  is  known  to  be 

g{riQ  t),o=mc,T),c)  do 

where  X(7)  = C (arbitrary  class  of  functions)  and  r has  the 
assumed  form  r(C,  7)  = x(7)  + P(T)C;  P = PT.  Substituting 
r into  (10)  yields  the  fixed-point  boundary  value  problem 
(thus  bypassing  the  difficult  two-point  boundary  value 


Experimental  Results 

(8) 

v ' Given  are  the  following  observations: 

*i°(0  = *i(0-M0 

*1  °(0  = *1  (0  ~ xt(t)6  - (,  (/) 

(9)  where 

6 = 18.56,  PRF  = 640,  / € [0,  5J , and  T = 5 seconds. 

alue  The  noise  sources  6| (/)  and  e2(t)  are  assumed  to  be 
The  independent  and  normally  distributed,  A'lfO,  o,1)  and 
(Va(0,  OjJ),  respectively.  An  initial  spectral  velocity  error 
of  5 lines  is  assumed  (i.e.,Xj(0)  = 5 -*■  92.8  yd/s  error).  The 
(10)  actual  velocity  *|(/)  is  to  be  5000  yd/s  for  all  time,  and 
the  actual  range  is  described  by  x,  (t)  = x,  (0)  + x,  (/)/  £ 
the  10  000  + 5000/  yards.  The  measurement  noise  error  vari- 
ting  ances  are  to  be  (0,  0)(no  noise  case),  (10*,  101),  and  ( 104, 
ilem  I04  ),  respectively.  Thus  the  ideal  estimation  vector  f,  if  all 
alue  measuiemcnt  error  could  be  suppressed,  would  be  x,(f)  *= 
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Fig.  4.  Optimal  giinPjj. 


o i j j » J 


x,  °(r)  and  x2(t)  = 0.  The  choice  of  W will  determine  the  estimate  requires  minimizing  both  errors  with  respect  to  the 
relative  importance  associated  with  minimizing  ej  (range  measurement  covariance  matrix,  or  in  this  embodiment,  it 
error)  and  €j  (range-rate  error)  v/hich  are  and  will  be  assumed  that  = Wj,  since  o, 1 = o2  2 , is  optimal, 
dependent,  respectively.  Thus  a range-dependent  estimate  It  can  be  noted  from  Figs.  2,  3,  and  4 that  the  optimal 
would  imply  >Wit  whereas  the  proposed  optimal  niter  matrix  P(t)  is  sensitive  to  the  choice  of  H'.  This 
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the  original  hypotheses  that  an  estimate  based  on  both 
range  and  range-rate  data  weighted  ay  an  inverse  convari- 
ance  relationship,  will  be  superior  to  a weighted  range 
estimate  only.  A highly  accurate  range-rate  estimate,  in 
terms  of  resolving  pulse  Doppler  ambiguous  data,  results. 
The  achieved  range  estimate  may  be  used  to  augment  more 
classical  pulse  delay  ranging  methods. 
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• FIGURE 


1 OPEN"LP:  " FOR  UUIIUI  AS  FILE  1 
10  DIM  G<4,  3)  > X ( 101 )«  Z(101) 

25  DIM  0(4,  50),  S<4> 

•30  LET  A=0:  LET  13=0:  LET  C»0 

40  LET  11=0:  LET  D=  18.  56:  LET  T*0:  LET  H«.  05 
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50 

55 

60 

65 

100 

105 

no 

115 

150 

155 

160 

165 

200 

220 

250 

255 

260 

265 

280 

300 

310 

320 

330 

350 

355 

360 

365 

400 

405 

420 

425 


PR  I NT  " M I SS I ON  TIME"!  SOURCE:KAIM\N  FILTER 

PRINT  #1,  "MISSION  TIME"* 

INPUT  NO  j 

PRINT  #1, NO 

PR I NT "BURST  TIMES  AND  SCALE  FACTOR"! 

PRINT  01, "BURST  TIMES  AND  SCALE  FACTOR"! 

INPUT  TO!  Tl*  S 
PRINT  01,  TOi  Tli  S 

PRINT"  INITIAL  RANGE.  . RANGE  RATE.  . . ACCELERATION"! 

PRINT  01, "INITIAL  RANGE  . RATE  RATE  . ACCELERATION"! 

INPUTSO;  Si!  S2 
PRINT  01,  SO!  Sli  S2 
LET  X=1 : LET  1=0 
LET  RO=SO: LET  R1=0 

PRINT"SIGNAL  TO  NOISE.  . RANGE.  . RANGE  RATE"! 

PRINT  01, "SIGNAL  TO  NOISE  RATIO  . RANGE  . RANGE  RATE" 

INPUT  VI ! V2 
PRINT  01,  Vl»  V2 

LET  N5=0: LET  N6=0: LET  N7=0: LET  N8-0:  LET  N9-0 
LET  V1=10^(V1/10): LET  V2«10~ < V2/10) 

FOR  S5=l  TO  3 
LET  Z=S0: GO  TO  1000 

PRINT  01,  "RESCALE  ECHO"! S/< Vl~2) l 1/(V1^2)!  ":  : "!  S/(V2~2>1  1/<V2~2> 
PRINT"VW1.  . . . VW2.  . . . VOBS" 

PRINT  01, "VW1  . VW2  . VOBS"! 

INPUT  W3,  W9,  W7 

PRINT  01,W8;W9;W7 
PR I NT "RESCALE"! 

PRINT  01, "RESCALE"! 

INPUT  S3,  S9,  S7 

PRINT  01,  S8,  S9,  S7 


500  PRINT  ’ -tHHHHHHt- 

600  PRINT  01: PRINT  01: PRINT  01 

700  PRINT  01,  "TIME  RANGE  ERROR  LINE  ERROR  GAIN  K1 

720  PRINT  01 

800  FOR  1 = 1 TO  NO/.  05 

900  REM  TARGET  NOMINAL 

910  LET  R0=S0+T*Sl+T#T*S2/2 

920  LET  R1=S1+T*S2 

1000  REM  TARGET 

1020  LET  N1=0: LET  N2=0 

1040  FOR  K=1  TO  12 

1060  LET  N1=N1+RND( I ) : LET  N2=N2+RND< I ) 

. 1080  NEXT  K 

* 1100  REM  NOISE  GEN 


GAIN  K2 


1200  LET  Nl=Nl-6: LET  N2=N2-6 

1210  LET  N5=N5+1 : LET  N6=N6+N1 : LET  N7=N7+N2:  LET  N8»N8+N1/S2:  LET  N9-N9+N2^2 

1220  LET  W=1 : LET  WJ=W7:LET  W2=WS: LET  W3=W9 

1240  IF  TCTO  THEN  1300 

1260  IF  T>T 1 THEN  1300 

1270  LET  R2=R0 

1280  LET  W=S: LET  W1=S7*W7:LET  W2=S8*W8: LET  W3®S9#W9 
1300  LET  RO=RO*< 1+W*N1/(V1A2) ) 

1320  LET  R1=R1*< 1 +W*N2/ < V2^2 > ) 

1340  IF  I>0  THEN  2000 

1360  GO  TO  330 

2000  REM  KALMAN  GAINS 

2020  LET  K 1=0: LET  K2=0: LET  K3»0 

7040  GOSUB  8000  “38" 

■'60  ITT  0(1.  1 >=F1:  LET  G<  1 , 2>=F2:  LET  G(1,3)=F3 


2100  GO^O-  6000 

2120  LET  G(2,  1 )**F1:  LET  0(2#  2>»F2:  LET  G<2, 3)-F3 
2140  LET  K1=H*F1/2:LET  K2-H*F2/2: LET  K3-H#F3/2 
2160  GOSUB  8000 

2 ISO  LET  G(3,  1 ) *=F  1 : LET  G<3,  2>«F2:  LET  G(3,3)=F3 
2200  LET  K1=H#F1 : LET  K2=H*F2:  LET  K3=H*F3 
2220  GOSUB  SOOO 

2240  LET  G<4,  1 >=F1:  LET  G<4,  2>=F2:  LET  G(4,3)=F3  H 

2260  LET  A=A+H*<G<1,  1)+2*G<2,  1)+2*G<3, 1)+G<4» l))/6 

2280  LET  B=B+H*<G< 1. 2)+2*G<2,  2)+2*G<3,  2>+G<4, 2) >/6 

2300  LET  C=C+H*  ( G < 1 , 3 ) +2*G  (2,3)  +2*0  < 3,  3 ) +G  < 4,  3 > ) /6 

2560  IF  ABS<A)>1E20  OR  ABS<B)>1E20  OR  ABS<C)>1E20  THEN  7000 

3000  REM  STATE  ESTIMATIONS 

3020  LET  K 1=0: LET  K2=0 

3040  GOSUB  6000 

3060  LET  G<  1,  1 )=F1:  _£T  G<1,2)**F2 
3080  LET  Kl=H*Fl/2: LET  K2=H*F2/2 
3100  GOSUB  6000 

3120  LET  G(2»  1 )=F1 : LET  Gi2,  2)=F2 
3140  LET  Kl=H*Fl/2: LET  K2=H*F2/2 
3460  GOSUB  6000 

3480  LET  G<3,  1 )»F1:  LET  G<3,2)»F2 
3500  LET  K1=H*F1 : LET  K2=H*F2 
3520  GOSUB  6000 

3540  LET  G<4, 1 )=F1: LET  G(4,  2)=F2 

4000  LET  Z=Z+H*<G<1,  1)+G<2,  1)*2+G<3,  1>*2+G<4,  l))/6 

4020  LET  X=X+H*<G< 1.  2)+G<2, 2)*2+G<3,  2>*2+G<4,  2> )/6 

4040  IF  ABS<  Z )>1E20  OR  ABS<X)>1E20  THEN  7000 

4500  LET  T~T+.  05 

4600  LET  T5=I-INT< I/4)*4 

4620  IF  T5O0  THEN  4800 

4700  PRINT  X»  A/Wl  # B/Wl 

4710  PRINT  #1,  I*H  , Z~R2  , X , A/Wl  # B/Hl 

4715  LET  S4=INT (1/4) 

4720  LET  Q(S5. S4)-X 

4800  NEXT  I 

4810  LET  T=0: LET  A=0: LET  B»0: LET  C=0: LET  X«0: LET  Z-SO 

4320LET  X=1  : LET  1=0 

4830  NEXT  S5 

4840  LET  S6=0 

4350  FOR  Sl=l  TO  3 

4860  FOR  S2=l  TO  25 

4870  IF  S6>=ABS<Q(S1,  S2> > THEN  4890 
4880  Ltr  S6=ABS ( Q< SI  S2) ) 

4890  NEXT  S2 
4900  NEXT  SI 
4910  FOR  Sl=l  TO  3 
4920  FOR  S2=l  TO  25 

4930  LET  G<S1, S2)=Q<S1, S2)/S6  ‘NORMALIZED  SOECTRAL  LINES 
4940  NEXT  S2 
4950  NEXT  SI 
4970  STOP 

4980  PR  I NT  <tl : LET  N6=N6/N5:  LET  N7=N7/N5:  LET  N8=N8/N5-N6:  LET  N9-N9/N5-N7 
4990  PRINT  i)l.  "MEAN  AND  VARIANCE.  . . RANGE "»  N6i  N8l  "RANGE  RATE"!  N7j  N9 
5015  PRINT  ttl: PRINT  ttl 

5020  PRINT  Ifl,  " SPECTRAL  LINES  IN  ERROR" 

5025  PRINT  01, TAB(30)> "0" 

5030  FOR  S=1  TO  25 
5040  MAT  S=ZER 
5050  LET  I =~2 
5060  FOR  M=1  TO  3 
5080  FOR  J=1  TO  3 
5090  IF  J=5(l)  THEN  5200 

5100  IF  J=S< 2 ) THEN  5200  39 

5120  IF  Q<J,  SX=I  THEN  5200 


52c\»  ...XI  J 

5210  LET  S<M)=>I0:  LET  I«-2 
5220  NEXT  M 

5400  FOR  J=1  TO  3 * . 

5410  LET  S< J)=30*Q(S<J),  S>+30 

5420  NEXT  J 

5500  PRINT  01,  S*.  2iTAB<S<3))i  "*"»  TAB(S(2)  )l  M#,,j  TAB(S(  1 ) ) I 

5600  NEXT  S 

5700  00  TO  100 

6000  REM  STATE  FUNCTIONS 

6020  LET  F1=R1+A*<R0~<Z+K1 ) )/Wl+D*( X+K2) 

6040  LET  F2=B-»(R0-(  Z+Kl ) )/Wl 

6100  RETURN 

7000  PR I NT "ERROR  DETECTED" 

7005  PRINT  #1. "ERROR  DETECTED" 

7020  PRINTI;  Zj  Xi  A;  B;  C;  W2i  W3i  W1 

7025  PRINT  Itl , Zi  Xi  A;  Bi  Cl  W2i  W3i  W1 

7040  GO  TO  30 

8000  REM  KALMAN  FUNCTIONS 

8010  LET  Fl«2*<B+K2)*D-< (A-Kl >^2>/Wl+W2 

8020  LET  F2S=(  C+K3)  #D~( A+Kl  )#<B+K2)/W1 

8030  LET  F3=W3-< (B+K2)^2)/W1 

8040  RETURN 

9900  CLOSE  1 

9999  END 
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BURST  TINES  AND  SCALE  FhCTORI  OPJ.N"KB:  " FOR  OUTPUT  AS  FILE  1 
10  DIM  G ( 4 , 3 ) » X ( 1 0 1 ) # Z < 1 0 1 ) 

25  DIM  Q<4,  50).  S<4) 


30  LET  A=0: LET  B=0: LET  C=0 

40  LET  1 1=0:  LET  D=  18.  56:  LET  T-0:  LET  H-.  05 

50  PR I NT "MI SSI ON  TIME"* 

55  PRINT  #1.  "MISSION  TIME"* 

60  INPUT  NO 

65  PRINT  JM.N0 


SOURCE:  STEADY  STATE 
KALMAN  FILTER 


100  PR I NT "BURST  TIMES  AND  SCALE  FACTOR"* 

105  PRINT  01.  "BURST  TIMES  AND  SCALE  FACTOR"* 

110  INPUT  TO*  T1 * S 

115  PRINT  01.  TO*  Tl*  S 

150  PRINT" INITIAL  RANGE.  . RANGE  RATE.  ..  ACCELERATION"* 

155  PRINT  01, "INITIAL  RANGE  . RATE  RATE  . ACCELERATION"* 

160  INPUTSO* SI* S2 

165  PRINT  01,  SO*  SI*  S2 

200  LET  X=1 : LET  1=0 

220  LET  R0=S0: LET  R1=0 

250  PR  I NT  "SIGNAL  TO  NOISE.  . RANGE.  . RANGE  RATE"* 

255  PRINT  01,  "SIGNAL  TO  NOISE  RATIO  . RANGE  . RANGE  RATE" 
260  INPUT  VI* V2 


265  PRINT  01,  VI  * V2 

270  LET  N5=0: LET  N6=0:  LET  N7=0:  LET  N8*0: LET  N9-0 
300  LET  V1=10~<V1/10):  LET  V2=10/'< V2/10) 


310  Fur  S5=l  TO  3 

320  LET  Z=S0: GO  T0  1000 

330  PRINT  01,  "RE  SCALE  ECHO"*  S/<V1'S2)<  1/(V1^2>*  : "*  S/(V2~2>*  1/<V2*2> 

350  PRINT"VW1.  . . . VW2.  . . . VOBS" 

355  PRINT  01,"VW1  . VW2  . VOBS"* 

360  INPUT  US,  W9.  U7 

365  PRINT  01,W3*W9*W7 

400  PR I NT "RESCALE"* 


405  PRINT  01, "RESCALE"* 

420  INPUT  38,  S9,  S7 

425  PRINT  01,33,  S9.S7 

500  PRINT  "*#***■**#****#•" 

600  PRINT  01: PRINT  01: PRINT  #1 

700  PRINT  01,  "TIME  RANGE  ERROR  LINE  ERROR  GAIN'Kl  GAIN 

720  PRINT  01 


800  FOR  1 = 1 TO  NO/.  05 
900  REM  TARGET  NOMINAL 

910  LET  RO=SO+T*Sl+T#T*S2/2 
920  LET  R1=S1+T*S2 
1000  REM  TARGET 


1020  LET  N1=0: LET  N2=0 
1040  FOR  K=1  TO  12 

1060  LET  N1=N1+RND< I ):  LET  N2=N2+RND<I) 

1030  NEXT  K 

1100  REM  NOISE  GEN 

1200  LET  N 1 =N1— 6: LET  N2=N2-6 

1210  LET  N5=N5+1 : LET  N6=N6+N  1 : LET  N7=N7+N2:  LET  N8=N8+N1/S2:  LET  N9=N9+N2^2 

1220  LET  W=1 : LET  W1=W7:LET  W2=W8: LET  W3=W9 

1240  IF  T<TO  THEN  1300 

1260  IF  T>Ti  THEN  1300 

1270  LET  R2=R0 

1230  LET  W=S:  LET  W1=S7#W7:  LET  W2«S8*W3:  LET  W3»S9#W9 
1300  LET  R0=R0*  ( 1 +W*N  1 / < V 1 *2 ) ) 

1320  LET  R 1 =R  1 ■**■  < 1 +W*N2/  ( V2^2 ) ) . 

1340  IF  I>0  THEN  2000  41 


2000  REN  KALMAN  GAINS 
2020  LET  B=SQR<W1*W3) 

2040  LET  A=SQR(Wl*<2*D*B+W2> ) 

2060  LET  C=A*B/<B*W1 ) 

2560  IF  ABS<A)>1E20  OR  ABS(B)>iE20  OR  ABS<C)>1E20  THEN  7000 
3000  REM  STATE  ESTIMATIONS 
3020  LET  K1=0: LET  K2=0 
3040  GOSUB  6000 

3060  LET  G<  1 » 1 )=F1 : LET  G<1#2)=F2 

3080  LET  Kl=H*Fl/2:  LET  K2=H*F2/2  TT 

3100  GOSUB  6000 

3120  LET  G<2, 1 )=F1:  LET  G<2, 2)=F2 
3140  LET  Kl=H*Fl/2:  LET  K2=H*F2/2 
3460  GOSUB  6000 

3480  LET  G<3,  1 >=F1:  LET  G(3,2)=F2 
3500  LET  K1=H*F1: LET  K2=H*F2 
3520  GOSUB  6000 

3540  LET  6(4,  1 >=F1: LET  G<4, 2)=F2 

4000  LET  Z=Z+H*(G<  1»  1 >+G<2#  1 >*2+G<3#  1 >*2+G<4#  1 ) >/6 

4020  LET  X=X+H*(G<1.  2)+G(2,  2>*2+G<3»  2>*2-M3<4#  2)  )/6 

4040  IF  ABS<  Z )>1E20  OR  ABS<X)>1E20  THEN  7000 

4500  LET  T=T+.  05 

4600  LET  i5=I-INT<I/4>*4 

4620  IF  T5O0  THEN  4800  - 

4700  PRINT  I#H;  Zl  Xl  A»  Bl  Cl  W2l  W3i  Wl 

4710  PRINT41#  I*H  , Z-R2  «X  » A*W1  » B*W1 

4720  LET  S4=INT ( 1/4) : LET  Q<S5,  S4)«X 

4800  NEXT  I 

4310  LET  T”0: LET  A=0: LET  B»0: LET  C«0: LFT  X-0: LET  Z-SO 
4820LET  X=1  : LET  1=0 

4330  NEXT  35 
4340  LET  S6-0 

4350  FOR  SI =1  TO  3 ' ' 

4860  FOR  S2=l  TO  25 

4370  IF  S6>=ABS(Q(Si. S2) ) THEN  4890 
4830  LET  S6=ABS(Q<SI,  S2) ) 

4890  Nr'  ' S2 
4900  NE.,  T SI 

4910  FOR  Sl=l  TO  3 
4920  FOR  S2=l  TO  25 

4930  LET  Q(S1, S2)=Q(S1» S2)/S6  (NORMALIZED  SOECTRAL  LINES 
4940  NEXT  S2 
4950  NEXT  SI 
4970  STOP 

4980  PRINT  #1 : LET  N6=N6/N5:  LET  N7=N7/N5:  LET  N8=N8/N5-N6:  LET  N9=N9/N5-N7 
4990  PRINT  #1,  "MEAN  * VARIANCE.  . RANGE" l N6i  N8i  "RANGE  RATE.  . "}N7iN9 
5000  PRINT 
5010  r"  ; i'  0* 

5020  PRINT  «i/'  SPECTRAL  LINES  IN  ERROR" 

5025  PRINT  41  * TAB ( 30 ) l "0" 

5030  FOR  S=1  TU  25 

5040  NAT  S=ZER 

5050  LET  I =-2 

5060  FOR  M = 1 

5080  FOR  J=1  iU  3 

5090  IF  J=S<1)  THEN  5200 

5100  IF  J=S<2>  THEN  5200 

5110  IF  J~S ( 3 ) THEN  5200 

5120  IF  Q ( • J < CIK=I  THEN  5200 

5140  LET  I=G<J, S):  LET  IO=J 

5200  NEXT  J 

5210  LET  S(M)=I0: LET  I»-2 

5220  NEXT  M 

5400  FOR  J=1  TO  3 

> -T  -r  < S)+30 


PRINT  #1.  S*.  2 > TAD < SO)  )f  '’*"1  TABOO)  >1 
NEXT  S 
GO  TO  1 00 

REM  STATE  FUNCTIONS 
LET  F 1 =R  1 + A* < R0- ( Z+Kl ) )/Wl+D*< X+K2) 
LET  F2=B*<R0-< Z+Kl ) )/Wl 
RETURN 

PR I NT "ERROR  DETECTED" 

PRINT  #1, "ERROR  DETECTED" 

PRINTIi  Zi  Xi  A i Bi  Ci  W2 1 W3)  W1 

PRINT  #1*  Z i Xl  A i B)  Cl  W2 < W3»  Wl 
GO  TO  30 
CLOSE  1 
END 


'♦"VfAB^Sii)  )i  n+n 
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ON  THE  COMPUTATION  OF  KALMAN  GAINS 

Fred  J.  Taylor 

Department  of  Electrical  Engineering,  University  of  Texas  at  El  Paso,  El  Paso,  Texas  79968, 
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( Received  13  May  1974) 

Abstract-  -Modem  filtering  methods  often  require  high  order  matrix  differential  equations  to  be 
solved.  Standard  numerical  methods  are  traditionally  slow  and  prone  to  be  unstable.  A numerical 
approach  to  the  problem  of  computing  the  Kalman  gain  matrix  is  developed  which  is  both  numeric- 
ally efficient  and  stable.  If  a piecewise  approximation  of  the  Kalman  gain  matrix  is  desired, 
efficiencies  of  many  orders  of  magnitude  can  be  realized. 

INTRODUCTION 

Contemporary  systems  literature  is  rich  in  the  study  of  minimal  variance  filtering  theory 
as  applied  to  linear  constant  coefficient  differentia!  dynamic  system  corrupted  by  stationary 
white  noise[l-3]. 

The  high  level  of  activity  in  this  area  has  produced  numerous  papers  on  the  utility,  as  well 
as  the  dangers,  of  filtering.  However,  in  the  embodiment  of  literature,  little  attention  has 
been  given  to  the  numerical  problems  associated  with  computing  the  required  tin. : varying 
matrix  gains  (Kalman  gains).  Such  problems  are  usually  treked  through  neo- classic 
Runge  Kutta,  Milne,  Adams,  etc.  methods.  They  are  often  slow.  This  can  sometimes  be 
forgiven  if  a non-real-time  filtering  is  sought.  Besides  being  slow,  they  are  inherently 
unstable.  This  cannot  be  tolerated  in  most  applications.  Therefore,  a computationally  fast 
and  stable  algorithm  shall  be  sought. 

Problem 

Let  a n dimensional  message  model  be  given  by 

*(0  = Fx(f)  + GM  t)  (1) 

with  r observations  given  by 

z(t)  = tfx(f)  + p(r).  (2) 

Let  the  white  noise  processes  be  defined  as  usual. 

£(x(0))  = x0;  £(w(f))  = £(i>(0)  ~ 0 
var(x(0))  - ^(0);cov(x(0)wr(t))  = 0 
cov(w(f)wr(i))  = K,<5(f  — t)  (3) 

cov(i)(t)pT(T))  = Fc(5(i  - t) 

COV(wff)t’r(T))  = COV(p(f)wT(l))  = 0. 

105 


« 


106 


Fred  J.  Taylor 


''lie  minimal  variance  estimate  of  x(t),  say  £(f).  is  known  to  satisfy  [3] 

x(t)  - Fi(t)  + K(t)[z(t)  - (4) 

*«)-  W)/frK,-‘  (5) 

where  ;?(f)  is  the  extinction  error  (i.e.  j?(f)  — x(f)  - x(f))  and  V^(f)  is  the  error  covariance 
matrix  satisfying 

VM  = FVt(t)  + ^{t)FT  - Vt(t)HTV- 'HVM  + GVwGt  (6) 

f,(0)  = K,(0). 


The  heart  of  the  filter  is  quantifying  the  error  covariance  matrix  K4(fX  for  t > 0.  This  is 
in  general  a non-trivial  numerical  problem-  However,  a recent  work  of  Davison  and 
Maki[4],  with  improvements  by  Taylor[5],  can  be  adapted  to  provide  a rapid  stable 
solution  of  equation  (6).  It  utilizes  an  approach  suggested  by  Sage[3]  (but  also  discouraged 
by  that  author)  which  interprets  equation  (6)  as  a 2 n x 2n  first  linear  differential  system. 

Consider,  for  N = 2n,  a N dimensional  vector  {(f)  satisfying 

[—FT  1 1 

GVjG*^ F J * (7) 

N x N 


This  solution  of  equation  (7)  is  characterized  by  the  matrix  exponential 


exp(#f) 


_ ^11 

_<t>n 


(0«*«  ! 


J 


N x N 


(8) 


It  can  also  be  shown  that 

W)  * t t0)  + <t>22  (f,  «o) ^i(O)] [<£,,(*,  t0)  + 0I2(r,  fo)^(0)]~‘.  (9) 


Therefore,  computing  Vx  lias  been  converted  into  a problem  of  computing  the  n x n 
matrices  <f>,j(t\  i = i2J  *■  1,2.  The  following  algorithm  can  be  used  to  efficiently  produce 
those  desired  matrices. 


Numerical  solution 

The  modified  Crank-Nicholson  matrix  approximation  is  given  by  [4, 5] 

np(0th)  = C + 0(hi) 

where 

C = [/  " hMf 2 + h2£ 712]- ‘ [/  f m/2  + h20t2/ 12]  (10) 
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where  h is  a scalar  (step  size)  and  Ofh*)  denotes  a fifth  order  of  error  accuracy.  (Runge-Kutta 
RK4  methods  provide  only  fourth  order  accuracy).  The  commutative  property  of  the 
fundamental  state  transition  matrix  exp  ( 9Ht ) admits  the  generation  of  ^(t),  l • \£ 
y-  1.2,  sequentially  since 


exp(dth).—  C 
exp(#2h)  — C 2 

(ID 


sxp(dfT)  = CN. 

For  some  prespecified  step  size  h,  suppose  T =»  2 kh.  A binary  coded  scheme  to  produce 
the  required  powers  of  C,  namely 

C2  « C*  • C* 

CJ  - C2Cl 
C4  = C2  • C2 
etc. 

would  require  2*/V3  multiplicative  operations.  If  2*  » N2,  it  is  shown  in  (5)  that  from  the 
application  of  Bocher’s  formula,  this  can  be  reduced  to  only  2kN2  multiplicative  operations. 
Bocher's  formula  yields  the  following  results  :f 

(i)  Let  C\  C2, . . . CN~ 1 be  computed. 

(ii)  Define 


ot(N  - 1, 0)  «=  T, 

a(N  - 2, 0)  - («(N  - 1, 0)  T,  + T2)/ 2 


<*(0,0) 


N-  1 


(I 


«(i,0)7J  + Tn)/N 


where  7j  = trace  (C*),  i = I, . . . , N — 1. 


t Bocher's  fomula  is  erroneously  transcribed  in  Ref.  [d].  It  is  derived  in  Appendix  A. 
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(iii)  Define  for  m > 0 

<x(N  - l,m)  - *(N  - 2,  m - 1)  - a(N  - 1,0)«(N  - 1,  m - 1) 
a(N  - 2,m)  » «(/V  - 3,m  - 1)  - *(N  - 2,0 )«(N  - I, m - 1) 


ot(0,  m)  =*  0 - a(0, 0 )a(N  - 1,  m - 1) 

(iv)  Then 

s-i 

CN*m  = £ a(i,  nt)C‘,  0 £ m Z 2k  - N. 
i»o 


Once  (i)  and  (ii)  have  been  precomputed,  all  successive  powers  of  C can  be  generated  recur- 
sively. Besides  realizing  a speed  improvement,  the  entire  program  could  be  effectively 
embedded  into  the  now  available  microprogrammable  minicomputers.  The  . ' o posed 
algorithm  would  require  N3  + N2  memory  word  locations  to  support  the  recursive  opera- 
tion. It  is  interesting  to  note  that  the  developed  algorithm  does  not  require  production  of 
the  eigenvalues  of  C.  Therefore,  one  of  the  main  objections  to  fundamental  solution  based 
techniques,  namely  solving  the  characteristic  polynomial  det  - C)  - 0,  is  not  an  issue. 
The  solution  technique  proposed  is  outlined  in  Fig.  1. 


INITIALIZE 


Nh  « I 


RECURSIVE  ALGORITHM 


On  the  compulation  or  Kalman  gains 


109 


Heuristic  extension 

Kalman  gains  are  usually  most  active  near  t = 0.  As  t -»  co,  K(f)  tends  exponentially  to 
some  steady-state  value  provided  the  plant-observer  pair  are  stable.  Therefore,  it  would 
seem  reasonable  that  a variable  step-size  algorithm  could  be  developed  which  would 
accelerate  the  solution  process.  The  variable  step-size  algorithm  should  give  a dense  time 
domain  cover  where  K(t)  is  most  active  (i.e.  t =s  0)  and  be  sparse  where  K(t)  is  inactive 
(K(0  -♦  oo ).  Consider  then  the  construction  of  an  algorithm  which  will  produce  a piecewise 
constant  approximation  to  K(t\  Let  this  suboptimal  gain  matrix  satisfy  the  following 
criterion : 


Criteria  ( piecewise  continuous  approximation ) 

Let  K*(t,)  be  a piecewise  continuous  approximation  of  Kit)  over  ^ t|+l!  where 

ll*a(fi)  - K(t)\\  Z e (12) 

e > 0.  Here  c serves  as  a prespecified  admissible  error  bound  on  the  approximation  process. 
A small  (large)  e would  result  in  a finely  (coarsely)  refined  approximation  of  K(t).  This  thesis 
can  be  effectively  accomplished  by  means  of  the  binaiy  coding  scheme  previously  mentioned. 
Consider  the  following  “variable  interval— c meteoric”  Kalman  gain  algorithm. 

Define  the  matrix  norm  of  a N x N square  matrix  A to  be 

II A||  = max  {|Ay|} 

i = 1, 2, . . . , JV  (13) 

j = 1, 2 N. 

Procedure 

No.  1 Choose  h sufficiently  small  so  that  0(hs)  error  is  tolerable 
No.  2 Compute  Cl  = exp  (&th) 

No.  3 Compute  C2'** ” = C2'.  C21 ; / = 1,  2,  3, . . . 

No.  4 Test:  IfHC2'1*"  — C2,||  < e 

(i)  if  true 

let  C21  2 exp  (0tt) 
for  2'*  <;  f < 2<i+  “ft 

(ii)  if  not  true 

reduce  search  interval  to  some  ? such  that  2‘h  <1  < 2,u  Vi  — S where 

0 < 6 < 2'h. 

Return  to  No.  4. 

Some  obvious  interval  reducing  methods  would  be  an  equal  interval,  dichotomous,  or 
Fibonacci  search  methods.  However,  from  a computational  efficiency  viewpoint,  the 
following  approach  has  proven  most  effective. 
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TIME  AXIS  SEGMENTATION 


t 


1+1 

1+1 

1+1 

1+1 

2 2H1  ««c 

1* 

I 

T 

7 

4 

3 

2 

1 

0 INDEX  r 

Fig.  2. 

Suppose  C21  has  been  computed  and  accepted  at  time  t = 2'h.  Suppose  further  that  the 
last  S computed  matrices,  namely  C2(,  C2(,~  C2,,_,),  are  stored  in  memory.  Compute 

C2<l*  ” as  in  No.  3.  If  it  fails  test  No.  4.  Reduce  the  search  interval  by  testing  C2'  sequentially 
against 

C2i+2<i-r)  m c2'.C2<,  f,;r  = {1,2 S}.  (14) 

Here  c2'+2<,  ')  can  easily  be  generated  by  direct  binary  operation  from  matrices  found  in 
memory.  The  reduced  interval  is  a monotonicaily  decreasing  sequence  over  the  index  set  r. 
It  is  characterized  in  Fig.  2 In  practice,  the  number  of  previously  computed  and  stored 
matrices,  indexed  by  S,  is  to  be  determined  experimentally.  Since  memory  requirements 
are  generally  considered  to  be  a secondary  goal  when  compared  to  computational  speed. 


Fig.  3. 


^ /e. 


t 


Ill 

cne  can  usually  be  optimistic  in  one's  choice  of  S.  If  it  should  turn  out  that  the  choice  of  S 
was  insufficient  to  support  the  piecewise  constant  approximation  process,  the  required 
powers  of  C can  be  synthesized  directly  from  binary  operations  on  C.  Therefore,  a solution 
is  always  recoverable.  The  suggested  approximation  process  is  depicted  in  Fig.  3. 

Steady-state  Kalman  gains 

Steady-state  Kalman  gains  can  now  easily  be  computed.  Using  a a test,  the  Kalman  gain 
at  some  time  t,  = 2'h  can  be  rapidly  computed  using  base-2  algorithm.  The  Kalman  gains 
are  assumed  to  have  non-oscillatory  behavior,  for  r,  sufficiently  large;  then  the  steady  value 
of  K(t)  may  be  assigned  the  value  K{tl+l)  if, 

l|K(*i+i)  — K(f.)||  < a,  t,  = 2'h,  a > 0.  (15) 

If  the  heuristic  algorithm  is  used,  which  maintains  a history  of  the  last  S “power  of  C” 
operations,  namely 

then  it  would  be  reasonable  to  assume  that  the  last  5 Kalman  gains  are  also  stored  in 
memory.  Let  the  following  n x n gain  matrices  be  found  in  memory 

{K(t,),  K(r,_ K(t,.s)}.  (16) 

Consider  the  Kalman  gain  matrix  to  have  obtained  a steady-state  value  if 

| £ K(t|_,)||  < <r/(S  + 1)  (17) 

(=0 

and  let  the  steady-state  value  of  K(t)  be  assigned  the  value  K(t,). 
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KALMAN  FILTER  gain  continuous  and  PIECEWISE  CONTINUOUS 
GRAPH  1 CONTINUOUS  ALGORITHM  RUN  TINE  294  NSEC 

GRAPH  2 EPSILON  = .01  RUNTIME  72  MSEC 

GRAPH  3 EPSILON  = .1  RUN  TIME  12  NSEC 

GRAPH  4 BASE  2 ALGORITHM  RUN  TIME  6 MSEC 


Fig.  4. 
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Example  1 ( scalar  example) 

Find  the  Kalman  gain  associated  with  the  system 

x°  - — 0-5x  + w u £ t «£  2048 


z **  x + v 


cov(w(t),  w(t))  = 2S(t  - t) 
cov(t>(f),  t>(r))  = l/4<$(f  - t) 
cov(xO),  x(0))  = 0. 


The  solution  to  the  above  problem  can  be  found  in  Sage  (p.  244),  and  is 

fnt 


K(t)  = V3(t )H'V'1  - -1/2  + 1/2 


N/33ftanh|^< 


+ tanh 


-i 


The  solution  obtained  using  the  new  proposed  algorithm,  over  u <,  t ^ 2 048,  in  steps  of 
0-001  sec,  is  denoted  in  graph  1 of  Fig.  4.  It  was  always  accurate  to  within  8 decimal  places. 
It  required  294  msec  to  complete  the  solution.  Using  the  heuristic  technique  cited  in  the 
paper,  an  epsilon  of  e = 0-01  and  e = 0-1  were  tested  (see  equation  12).  They  are  graphs  2 
and  3 of  Fig.  4 respectively.  The  test  associated  with  e = 0-01  produced  an  excellent  piece- 
wise  constant  approximation  to  K(t)  in  only  72  msec.  Using  the  fastest  algorithm  possible, 
namely  the  base  2 algorithm,  an  approximation  of  K(t)  as  f oo  and  produced  in  only  6 
msec.  This  result  appears  as  graph  4 of  Fig.  4. 


Example  2 
Given 


where 


( ° 

1 

°) 

!°\ 

MO  = 

' 0 

0 

1 

|x(f)  4- 

0 

1-1 

-3 

-3/ 

u 

2(1)  = (100)x(0  + i’(0 


Vw=  1 

K = 1 


^i(O)  — f- 

The  Kalman  gain  matrix  K(t),  over  0 < f < 1,  was  computed  using  conventional  RK-4  as 
well  as  the  proposed  algorithm,  using  a step  size  h of  0-01  sec  (the  solution  obtained  using 
the  new  algorithm  can  be  found  in  Appendix  B).*  Using  a RK-4  approach,  a solution  was 
obtained  in  38,320  msec.  Using  the  new  algorithm,  a solution  was  obtained  in  only  16,312 
msec.t  Furthermore,  any  reduction  in  step  size  h caused  the  RK-4  method  to  diverge. 
However,  the  proposed  algorithm  was  found  to  be  stable  and  independent  of  the  choice  of  h. 


* Appendix  B available  from  author  on  request. 

t 1 5,455  msec  where  needed  to  compute  Cl,  L = 3, 4, . . . , 1 00 ; 857  msec  where  needed  to  initialize  the  problem. 
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If  the  order  of  a system  is  increased  frotr.  n , to  «2,  there  would  be  a resulting  increase  in 
time  expended  on  the  computation  of  K(f)-  Suppose  it  took  K,  sec  to  compute  Vt(t)  for  a 
n,  x n,  system.  Then  computing  ki(f)  through  R-K  methods  would  require  approximately 


more  multiplications  to  generate  the  right-hand  side  of 

^(0  = FVM  + V,(t)FT  - V2(t) HTv-'mm  + GVwGt. 

Since  there  are 


more  integrals  to  solve,  it  would  take  approximately  (modulo  housekeeping  programming) 


seconds  to  compute  Vt(t)  for  the  n2  x n2  system.  The  proposed  algorithm  would  require 

sr-H' 

more  multiplications  to  construct 

C = "l  a,C' 

i = C 

and  approximately 


more  to  compute 

m = + tuvmitn  + *iaWor‘. 


If  it  took  K2  sec  to  compute  the  solution  of  a n,  x n,  system,  it  would  take  approximately 


seconds  to  solve  the  n2  x n2  problem.  Therefore,  there  would  be  a general  speed  improve- 
ment of  (for  n2/n , > 1) 


2/C, 

*2 


in  favor  of  the  proposed  method.  In  terms  of  3rd  order  benchmark  problem,  this  speed 
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improvement  factor  equates  to  approx.  4.  This  can  be  a meaningful  savings  when  the 
computation  times  become  long. 

However,  the  heuristic  algorithm,  previously  discussed  and  tested  in  example  1,  has 
been  shown  to  be  a great  saver  of  computer  time.  Several  orders  of  magnitude  may  be 
saved  if  a piecewise  continuous  approximation  of  K(f)  is  acceptable. 


CONCLUSIONS 

The  theory,  methodology,  and  supporting  examples  of  a new  Kalman  gain  matrix 
algorithm  have  been  presented.  The  results  to  date  are  most  satisfactory  in  terms  of  accuracy 
and  speed.  The  heuristic  algorithms  discussed  have  proven  to  be  a very  worthwhile  trade 
off  between  accuracy  and  speed.  Finally,  an  effective  approach  to  the  problem  of  estimating 
steady-state  gains  was  discussed  and  supported  with  an  example. 

The  developed  algorithm  and  sample  output  is  available  from  the  author.  It  is  coded  in 
Fortran  IV  and  appears  in  three  parts.  The  first  part  is  a program  dedicated  to  the  generation 
of  the  required  powers  of  C.  The  second  part  interprets  the  powers  of  C as  a Kalman  gain 
matrix.  The  third  part  is  a general  matrix  package.  The  program  currently  will  handle  a 
10th  order  system  but  can  easily  be  expanded  upward. 
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APPENDIX  A 

Bocher's  formula 

From  the  Cayley-Hamilton  Theorem,  a n x matrix  A satisfies 

A”  = a0i0I  + a10-4  + ...  + a „_2i0/T'2  + o/t"-1 

then 


A"*1  = A"  A = (ae0i0/  + ol10A  + . . . + aB_  1,0-4"  _1)-4 
= + ai,o-42  + ...)  + 

= (*0,0^4  + a i,o-4 1 + ..) -F  aN_  t,0(ao,of  4- #1,0-4 + i,o-4"  *). 


//V  / 
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Collecting  terms,  and  defining  A*+ 1 to  be 

A"*1  m ®o,i/  + «t.,^  + . . . + #»- 1 

one  obtains 

*0,1  = #»-  1,0  • #0,0 

*1.1  " *0,0  + #H-  1,0  ■ #t,0 

(, 

\ 

i. 


**-1,0  ■+■  #*-  1,0  • #*- l.o* 

Continuing  in  this  manner,  the  following  inductively  follows 
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TRANSIENT  RESPONSE  ANALYSIS  ON  MINI  COMPUTERS 
INTRODUCTION 

Controls  engineers  have  through  the  years,  found  the 

computer  to  be  u valuable,  if  not  indispensable  tool.  He 

has  found  ways  to  harness  the  power  of  both  analog  and 

digital  devices.  Hundreds  of  thousands  of  hours  have  been 

devoted  to  tho  study  of  control  systems  by  computer  methods. 

Simulation  has  become  the  rule  rather  than  the  exception. 

Many  numerical  integration  techniques  have  been  developed 

to  accomplish  the  required  simulation.  In  a recent  work 

of  J.  Reitman,  he  noted  [1]. 

"Historically,  the  control  system  engineers  developed  the 
simulation  methodology  as  an  adjunct  to  the  development  of 
analog  computers.  Now  extensive  use  Is  made  of  digital  and 
hybrid  digital-analog  computer  systems.  To  make  the  transition 
from  analog  to  digital  computers  easier,  a number  of  digital 
computer  simulation  languages  have  evolved:  Mimic  [2],  [3], 

Midas  [4],  Pactolus  [5],  CSMP  [6],  [7],  and  CSSL  [8]-[10]H 

These  simulations  are  performed  on  large  to  medium  size 
computers.  The  recent  availability  of  mini  computers 
however  has  had  very  little  impact  in  simulating  the  re- 
sponse of  large  dynamical  systems.  This  is  unfortunate 
when  one  considers  the  wealth  of  interactive  1-0  devices 
which  would  make  simulation  a highly  animated  experience. 
There  are  several  reasons  vhy  the  mini  has  fallen  short 
as  a simulation  tool.  Many  of  them  are  based  on  economics. 
However,  from  a technical  point  s*  view  it  can  be  noted 
from  those  who  have  attempted  to  do  system  simulation  on 
a mini,  become  aware  that  a 16  bit  word  is  often  too  small 
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to  avoid  the  "curse”  of  numerical  instability.  Here  a dou- 
ble precision  32  bit  word  (floating  point)  often  numerically 
truncates  the  answer  at  each  iteration  of  a numerical  inte- 
gration scheme  so  as  to  cause  the  answer  to  diverge  or  be- 
come a poor  representation  of  the  true  solution.  Of  course, 
high  level  languages  can  be  written  to  work  with  a 4 woTd 
(64  bit)  floating  point  format.  However,  for  the  purpose  of 
simulation  the  resultant  routine  is  prohibitively  slow.  To 
overcome  this  problem  a numerically  stable  and/or  speed,  an 
algorithm  is  presented  which  will  overcome  this  cited  numer- 
ical problem  of  conventional  integration  methods  and  thereby 
make  it  suitable  for  mini  computer  use.  It  is.  especially 
designed  to  efficiently  compute  the  response  of  a linear 
constant  coefficient  control  system  to  the  common  test,  inputs 
of  (1)  as  step  (2)  a ramp,  and  (3)  a constant  acceleration 
input. 

STATE  VARIABLE  MODEL 

Let  it  be  assumed  that  the  control  system  under  consi- 
deration satisfies  the  following  state  variable  equations: 


plant 


8(t)  a A x(t)  + bu(t)  : x(0)  ■ x 


with  observations 

y (t)  - Cx(t) 


where  "o"  means  d/dt  and  x(t)  and  y(t)  are  n and  m vectors 
respectively.  The  control  u(t)  is  to  be  considered  to  be 
one  of  the  following  conventional  test  inputs. 


"*ra 


U(t)' 


0 

C 

Ct 

ct: 


y(t)  ■ unforced  response 
(impulse  response) 
y(t)  ■ step  response 

y(t)  ■ ramp  response 

y(t)  ■ acceleration  response 


(3) 


The  proposed  system  is  diagrammed  in  Figure  1. 


Consider  for  the  moment  the  following  special  cases 

a)  u(t)  * 0 

then  the  system  equations  become 

8(t)  - A x(t),  y(t)  - C x(t)  (4) 

b)  u ( t ) - c 

defining  xn+1  (t)  * u(t)  « c , and  noting 

Vl  (t)  * °«  xn*l  " c 

the  state  equations  can  be  written  as 

2 ct) 


’°(t) 

A f b 1 0 | ol 

x(t) 

x(0) 

[x0J 

* 

- 1 

0 |0  * 1 f 0 

“T  T- 

xn*l(t) 

Vi'°) 

0 1 

« 9 ••  . 

Sn»3(t) 

J 

0 | 0 1 0 f 1 

“ T -J  - |~ 

0 I 0 * 0 1 0 

p 1 1 I . 

W*> 

xn*3Ct) 

m • 

xn*3C°> 

» « 

-•I 

Lci 

and 


(9) 


y(t) 


[C  0 0 0] 


-xCt) 

xn*lCt) 

xn*ZCt) 

xn*3(t) 

P 


The  equations  found  in  (a)  thru  (d)  are  called 


mented  state  equations”  and  can  be  generalized 


of  an 


(10) 


the  ”aug 
in  terms 


(i)  augmented  state  vector,  say  X 
(ii)  augmented  plant  matrix,  say  A 
(ii)  augmented  observation  matrix,  say  C 


such  that 


o 


xct)  - A X 

ft) 

(ii) 

y(t)  - C X 

(t) 

(12) 

Equations  of  the  form  of  this  can  be  solved  through 
direct  application  of  the  ’’state  transition  matrix”  or, 
as  it  is  often  called,  the  ’’matrix  exponential”, 

[11],  [12].  This  matrix  is  denoted 

exp  (At)  (13) 

defines  the  solution  of  (11)  to  be 


X(t)  - exp  (At)  x (0) 


(14) 


Several  methods  have  been  proposed  which  allow  the  user 
to  compute  the  required  matrix  exponential.  They  are: 

1.  Liou  method  [13] 

2.  (sI-A)  1 method  [14] 

3.  matrix  inversion  Lemma  method  [15] 

4.  Davison  method  [16] 


50 


It  is  the  Davison  method,  due  to  its  numerical 
stability,  which  is  particularly  attractive.  It  generates 
the  following  closed  form  representation  of  exp  (At)  over 
some  given  step  size  h,  namely. 

exp  (Ah)  ' (I  -■  hA  + h^A^)  * (I  * W\  + (15) 

T~  ~TT  T IT 

The  ability  to  define  the  matrix  exponential  over 
some  small  interval  h is  not  a handicap  in  that  it  is 
well-known  that 

exp  (A2h)  - exp  (Ah)  *exp  (Ah) 

.(16) 

exp  (A3h)  = exp  (A2h)  *exp  (Ah) 
and  so  on.  In  general,  X(*h)  can  be  computed  recursive- 
ly as  follows 

X(*h)  - exp  (A*h)X  CO 

- exp  (A(*  - l)h)  exp  (Ah)  X (0) 
recursive 

An  efficient  technique  using  Bochers  formula  has 
been  published  by  Taylor  [17]. 

However,  those  performing  simulation  on  a dedicated 
mini  are  usually  interested  in  resolving  the  trajection 
compactly  in  time  during  the  transient  period  and 
allowing  the  resolution  along  the  time  axis  "slip"  as 
the  solution  approaches  steady  state.  Of  course,  it  is 


assumed  that  the  amplitude  accuracy  will  be  preserved 
independent  of  the  time  axis  resolution.  The  algorithm 
proposed  is  extremely  useful  for  such  applications  in 
that  it  can  accelerate  along  the  time  axis  at  a very 
rapid  rate.  That  is,  suppose  the  current  solution  is 

x(£h),  where 

x(£h)  - exp  (A*h)  x (0) 
and  y(£h)  - C X 

If  exp  (A*h)  is  stored,  then  the  solution  as  2th  can  be 
obtained  by  simply  forming 

exp  (A2*h)  ■ exp  (A*h)  *exp  (A*h)  (18) 

and  generating 

X (2th)  - exp  (A2 Ah)  X (0) 

In  general,  after  M operations,  starting  at  x(0) , 

X(2^  ^h)  can  be  obtained.  Thus  the  time  axis  can  be 
scanned  in  base  2 . As  a byproduct , it  can  be  noted  that  for 
M small,  the  solution  is  highly  refined  near  t ■ 0.  As 
M increases,  (suppose  the  trajections  are  asymptomatically 
stable)  the  time  axis  in  the  steady-state  region  is 
coarsely  refined.  For  example,  consider  the  simple 
process  $c(t)  ■ -x(t),  which  for  h chooses  to  be  .1 
seconds,  has  the  response  x(£h)  ■ exp(-£h)xo  (see  Fig.  2) 
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.1 


Base  2 algorithm 

z 

Suppose  that  further  amplitude  resolution  is  desired. 
This  could  be  achieved  as  follows  (see  Fig.  2).  Suppose 
the  solution  currently  resides  at  £ = 8 (t  = .8)  which 
was  the  result  of  operating  on  the  previously  computed 
exp  (4£)  with  itself.  Suppose  exp  (2£)  remained  in 

2 

memory,  this  would  allow  the  user  to  compute  exp  (6 £) 
exp  (4£)  = exp  (2£)  and  so  on.  The  algorithm  present  in 
this  work  allocates  additional  storage  (called  the 
"push  - down"  stack)  to  allow  the  user  to  create  a more 
dense  solution  space  than  available  with  the  direct 
base  2 algorithm. 

One  last  feature  of  the  proposed  algorithm  shall  be 


developed.  From  the  theory  of  infinite  matricis,  it  can 
be  shown  that  the  steady  state  step  response  of  the 
original  system  (equation  1 and  2)  is  given  by 

y (Steady-State)  - C[I  - exp  (Ah))"1  bh  [ ] . (19 

Therefore,  if  one  is  simulating  a step  response, 
he  may  also  choose  to  resolve  his  answer  in  terms  of  a 
percent  of  the  steady  state  response.  That  is,  suppose 
8(t)  ■ -x(t)  + l(t)  , x (0)  » 0 
y (t)  - x(t) 

Therefore  y(t)  » 1 - exp(-t)  or  y (steady  state)  » 1. 
Also,  if  h *=  .01  then  the  steady  state  approximating 
equation  yields  an  answer  of  1.005008333  = 1.  If  the 
user  desires  the  amplitude  resolved  to  at  least  10%  of 
the  steady  state  value  then  he  would  require  that  no  two 
adjacent  amplitudes,  say  y(£h)  and  y(mh),  where  m is  the 
successor  of  i , differ  by  no  more  than  10%  of  1 , or  .1. 
Suppose  exp  (A*0 , exp  (A(*-l)),  through  exp  (A(t-r))  are 
stored  in  core.  One  would  test  a base  2 "jump"  to 
y(2fc).  If  J y (2 Jt)  - y(£)|>.l  then  generate  y(£*£-l)  = 
exp  (At)  *exp  (A(t-l))  x(0).  If  jy(*+£-l)  = y(£) | > .1, 
test  y(£)  against  y(£+£+2)  and  so  on  and  see  figure  3. 
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Example 

The  following  simple  example  shall  be  used  to  demonstrate  the 
salient  features  of  the  developed  algorithm.  A second  order  under-* 
damped  system  Is  considered  and  is  given  by 


i)  - +1.5^iSl  + j(t)  - l(t) 

dt2  dt 

z(0)  • 0 , dz(0)/dt  ■ 0 


11)  y(t)  - x(t) 

The  solution  Is  known  to  be  given  by 

y(t)  - 1 sin  + 60°) 

/3  * 

The  state  representation  of  the  above  .system  is 
x(t)  - 0 x(t)  - ® l(t)  ■ Ax  + bu(t) 

L-i  -i.sj  [i] 


dz(t)/dt 


, x(0)  - [0] 


y(t)  - [1  0]  x(t)  - Cx(t) 

The  augmented  state  variable  representation  which  simulates  a step 
response  is  (see  equation  (5)) 

fo  1 ol  Tol 


X(t)  - AX(0  - -1  -i.5  1 X(t>  , X(0)  - 0 


0 0 0 


y(t)  - CX(t)  - [loo]  X(t) 

The  developed  algorithm  requires  the  user  specify  matrices  /\,  C> 
and  the  vector  XC0> . If  a modulo  2 simulation  is  desired,  the  user 


must  supply 
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a)  step  size  Dt 

b)  terminal  time 

c)  set  the  size  of  the  push-down  stack  to  one  "1". 

If  a modified  modulo  2 simulation  Is  desired,  the  user  must  supply 

a)  step  size/kt 

b)  terminal  time 

c)  choose  desired  size  of  push-down  stack 

d)  choose  desired  percent  resolution 

e)  members  of  x(t)  to  be  resolved. 

It  should  be  noted  that  the  resolution  test  on  state  x^(t)  Is  based 
on  a given  percent  of  the  steady  state  value  of  that  state. 
Therefore,  only  those  states  which  result  in  a finite  steady  state 
value  should  be  considered  as  candidates  to  be  resolved  beyond  the 
modulo  2 resolution. 

The  problem  under  study  shall  be  approached  two  wayB.  A 
modulo-2  and  modified  modulo-2  solution  Bhall  be  presented.  For 
the  example  under  consideration,  the  steady  state  value  of  x^(t) 
and  x2(t)  (using  equation  (19))  was  found  to  be  .999928  * 1 and 
8.40455  x 10  -3  a 0 respectively.  The  state  x^(t)  shall  be  chosen 
to  be  resolved  with  15!?  of  its  steady  state  value.  That  is,  no 
two  successive  values  of  the  modified  modulo-2  simulation  will 
differ  by  at  most  .15.  The  results  of  these  two  simulations  are 
summarized  in  figure  4.  It  should  be  noted  that  modulo-2  algorithm 
is  an  accurate,  but  coarse,  representation  of  y(t);  whereas  the 
modified  modulo-2  algorithm  is  a vastly  superior  representation  of 
the  actual  y(t).  A more  highly  refined  algorithm  is  required  by 
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Increasing  the  size  of  the  push-down  stack  and  decreasing  the  per- 
cent resolution  value.  If  the  "knee"  about  the  point  of  maximum 
overshoot  is  to  be  further  resolved,  a special  routine  could  be 
written.  This  routine  would  giVe  x^(t)  additional  refinement 
whenever  the  slope  of  x^(t)  changes  signs.  That  is,  a local  minima 
or  maxima  is  known  to  occur  of  points  where  the  slope  of  that  once 
passes  through  zero.  With  respect  to  the  example  problem,  the 
region  between  sign  changes  of  X£(t)  (note  X2(t)  *r  dx^(t)/dt) , 
namely  2.4  ^ t <_  4.8  would  be  further  resolved. 

Besides  being  numerically  stable,  the  algorithm  was  found  to 
be  considerably  faster  than  conventional  numerical  integration 
formulas.  The  speed  increase  is  due  to  the  algorithm's  ability  to 
"leap  frog"  along  the  time  axis.  In  the  case  of  the  modified 
base-2  algorithm,  the  "leap  frogging"  moves  in  ever-increasing  step 
sizes  provided  the  percent  resolution  test  is  not  violated.  In  the 
considered  example  for  a basic  step  size  of  .01  seconds,  to  com- 
plete a solution  over  0 <_  t 10,  it  takes 

i)  1000  solution  steps  by  Runge-Kutta  methods 
il)  18  solution  steps  by  modified  base-2  methods 
iii)  9 solution  steps  by  base-2  methods. 

It  should  be  now  self-evident  why  the  algorithm  is  fast,  therefore, 

an  economic  design  tool. 
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DESCRIPTION  OF  SOFTWARE 


The  following  program  is  written  in  Extended  Basic  and  was 
implemented  on  a Hewlett-Packard  2100-$  minicomputer.  Its  use  was 
described  in  the  previous  section.  Briefly,  the  generation  of  G'pCfiS) 
(matrix  exponential  approximation) is  generated  between  statement  400 
and  530.  If  needed,  the  steady  state  value  of  x(t)  is  generated 
between  statement  645  and  685.  From  statement  890  to  930,  the 
state  y (£)  * £ exp  (/\£h)  X(0)  Is  produced.  If  an  error  tolerance 
is  requested,  it  will  be  tested  from  statement  940  to  990.  If  the 
error  tolerance  is  violated,  it  is  sent  to  statement  2000  for 
resolvement.  Statements  1110  to  1610  are  dedicated  to  restacking 
the  push  down  stack.  The  program  will  loop  back  to  900  until  the 
terminal  time  is  achieved.  Once  achieved,  the  program  will  process 
statement  1665  to  1780  to  complete  the  solution.  The  initial 
parameters  set  consisting  of  /\,  C>  and  X(0)  are  centered  as  data 
statements  located  at  and  beyond  statement  300. 
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LIST 

10  REN  THE  FOLLOVINO  DIMENSION  MUST  BE  CONSISTENT  WITH  THE  PROBLEM 
20  DIN  Xt3 1, Yt 31, Z12),V[2I, 012], 112 ] , EC2,21, FC2,2), 012,21, BI2,2J 
30  DIN  AC3,3],C[3,3},H(2,3],R[3,31,S(3,3),V(3,3) 

40  REN  DUNNY  ARRARYS 

50  DIN  TU00,10],Q[2501,U(10,2501,L110J,DU01 
55  PRINT  "DINENSION  OF  STATE  SPACE" 


60 

INPUT  N6 

65 

PRINT  "DINENSION  OF  AUQNENTED  STATE  SPACE" 

70 

INPUT  N 

60 

PRINT  "DIMENSION  OF  MESSAGE  SPACE?" 

90 

INPUT  N5 

100 

PRINT  "STEP  size  IN  SECONDS?" 

110 

INPUT  D 

120 

PRINT  "TERMINAL  TIME  IN  SECONDS?" 

130 

INPUT  F 

140 

PRINT  "IF  PLANT  IS  STABLE****RESPOND  1 

(ONE)" 

150 

INPUT  T9 

160 

IF  T9*l  THEN  190 

170 

PRINT  "PERCENT  RESOLUTION  DESIRED?" 

180 

INPUT  E 

181 

FOR  1=1  TO  N5 

182 

PRINT  "RESOLVE  Y<"| If")?  IF  SO  RESPOND" 

f lf"0  OTHERWISE" 

183 

INPUT  DID 

184 

NEXT  I 

190 

PRINT  "DESIRED  SIZE  OF  PUSH  DOWN  STACK? 

m 

195 

INPUT  N1 

200 

MAT  REAP  X 

205 

MAT  READ  A 

210 

MAT  READ  H 

230 

°R I NT  "INITIAL  AUGMENTED  STATE  VECTOR" 

240 

«T  PRINT  X 

2 c>0 

,^,INT  "AUGMENTED  PLANT  MATRIX" 

260 

MAT  PRINT  A 

270 

PRINT  "OBSERVATION  MATRIX" 

280 

MAT  PRINT  H 

290 

REM  LABLES  300  T0  390  ARE  RESERVED  FOR 

DATA  STATEMENTS 

3 00 

DATA  0,0,1 

310 

DATA  0,1,0 

320 

DATA  -1,- 1,1 

330 

DATA  0,0,0 

340 

DATA  1,0,0 

350 

DATA  0,1,0 

400 

REM  COMPUTE  C 

410 

MAT  R=  A*A 

420 

MAT  R= (D*D/12)*R 

430 

MAT  S= (D/2)*A 

440 

MAT  C=R+S 

450 

MAT  S=R-S 

460 

MAT  R= IDN( N,N) 

470 

MAT  S=  S+R 

480 

MAT  C=C+R 

490 

MAT  W: INV(R) 

500 

MAT  fix  W*C 
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500  MAT  Rs V*C 
510  MAT  CsR 

520  PRINT  "MATRIX  EXPONENTIAL  APPROXIMATION" 

521  GOTO  540 
530  MAT  PRINT  C 

540  REM  PERCENT  RESOLUTION  TEST 

550  IF  T9#l  THEN  700 

600  FOR  K=  1 TO  N6 

605  FOR  J=1  TO  N6 

610  LET  E(K,J)  = C[K,J1 

615  NEXT  J 

620  NEXT  K 

625  MAT  FsIDNt N6,N6) 

630  MAT  G=F-E 

635  PRINT  "IF  NATRIX  IS  SIQNULAR  ERROR  CODE  WILL  FOLLOW" 
640  PRINT  " REPEAT  PROBLEM  WITHOUT  ERROR  TOLLERANCE" 

645  MAT  Es INV(G) 

650  FOR  K=1  TO  N5 

655  LET  S=0 

660  FOR  J=1  TO  N6 

665  LET  SsHIK, J)*E[ JtN« ]+S 

670  NEXT  J 

675  LET  VtKJ  = S*D*.5 

680  PRINT  "APPROX  STEADY  STATE  VALUE  OF  Y<"fK|">s"jVtKl 

685  NEXT  K 
700  MAT  T=ZER 
710  MAT  Z-ZEH 
720  MAT  L=ZER 
750  MAT  R=C 
755  MAT  I=H*X 
760  FOR  K=1  TO  N5 
770  LET  U[K,1 1 = It  K] 

780  NEXT  K 
790  LET  Q[1]=D 
800  LET  Lt  1 1 = D 

810  REM  Ml  IS  THE  COMPLETED  SAMPLE  COUNTER 

820  Rl  . N2  IS  AN  EXHAUSTIVE  STACK  COUNTER 

830  rEM  N3  IS  A MODULO  10  COUNTER 

840  FOR  Ksl  TO  N 

850  FOR  L=1  TO  N 

860  LET  TtK,Ll=CCKfLl 

870  NEXT  L 

880  NEXT  K 

390  LET  M1=N2=1 

900  LET  ,'J3=0 

910  MAT  SsR*R 

920  MAT  Y=S*X 

930  MAT  1=  H*Y 

940  IF  T9#l  THEN  1100 

950  MAT  0=Z-I 

960  FOR  K=1  TO  N5 

965  IF  hVDCKl  THEN  990 

970  LET  SlsABS(O(Kl*lO0/VtXl) 

980  IF  SI  >s  E THEN  2000 

990  NEXT  K 

1000  MAT  Z=I 

1100  LET  TUL111+LCN3+1) 

1110  REM  RESTACX  DATA 
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• • 

1 129 

FOR  K=  1 TO  Ni-1 

I 

1139 

LET  L[Ni+l-Kl=L(Nl-K] 

1 

1140 

NEXT  K 

1150 

FOR  K=1  TO  Nl-1 

1160 

LET  K 1 = ( N 1 - K) * 1 0+1 « 00000E-04 

1170 

LET  Kl  = INT<Ki> 

1 180 

FOR  K3=l  TO  N 

J 

1190 

FOR  K4=l  TO  N 

1 200 

LET  T{Kl+K3fK41sTCKl-10+K3,K4i 

i 2 1 

NEXT  K4 

1220 

NEXT  K3 

1230 

NEXT  K 

1500 

FOR  K:1  TO  N 

] 

1510 

FOR  L=1  TO  N 

1520 

LET  T[K,L]:SCX,L) 

" 

1530 

NEXT  L 

1540 

NEXT  K 

i 

1550 

LET  M1=M1+1 

1560 

MAT  I=h*Y 

f , 

1570 

FOR  K=1  TO  N5 

' i 

1580 

LET  U{K,mirItK3 

1590 

NEXT  K 

1600 

LET  Q(M1]=T1 

- 

1610 

LET  L[1]=T1 

1650 

MAT  R=S 

1660 

IF  T1«F*2  THEN  $00 

' 1665 

LET  Qlll  = 0 

16'  3 

FOR  1=2  TO  Ml 

; 

1675 

LET  QtIJ  = Qm/2 

6o« 

NEXT  I 

1700 

PRINT  "IN  TUBULAR  FORM  THE  OUTPUT  STATES  (MESSAGE)  ARE" 

1710 

FOR  L=1  TO  Ml 

1720 

PRINT  "TIME";QIL1 

1732 

FOR  K=1  TO  N5 

1 17413 

PRINT  U(K,LJ 

1750 

NEXT  K 

1760 

NEXT  L 

1770 

PRINT  "FINIS" 

: 

1780 

STOP 

2000 

IF  Ml  >=  Ni  THEN  2040 

* 2010 

REM  SET  ALLOWABLE  STACK  PARAMETERS 

2020 

IF  M1-N3  <s  0 THEN  3003 

2040 

IF  N3  >:  Nl-1  THEN  40£0 

, 

2050 

FOR  K=1  TO  N 

' 

2060 

FOR  L=1  TO  N 

j 

2070 

LET  U=INT(10*N3+10'001) 

2075 

LET  S=0 

1 

i 

2080 

FOR  J=1  TO  N 

\ 

2085 

LET  S=T[¥+K,J1*TCJ,L1+S 

j 

2090 

NEXT  J 

• 

2095 

LET  RtK,L)sS 

2100 

NEXT  L 

2105 

NEXT  K 

, 2200 

MAT  S=R 

2210 

LET  N3=  N3+1 

2220 

GOTO  920 

• < 
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5000  PRINT  "STACK  DEPLETED  AT  STEP"fMl 
3010  PRINT  "LAST  ERROR  MAGN1TUTE  WAS", SI 

3020  PRINT  "DO  YOU  WISH  TO  CONTINUE  WITH  NEW  ERROR  TOLLER ANCE" 
3030  PRINT  " IF  SO  RESPOND  1C0NE)" 

3040  INPUT  F8 

3050  IF  F8:l  THEN  3100 

3060  STOP 

3100  PRINT  " ENTER  NEW  PRECENT  ERROR  " 

3110  INPUT  E 
3120  GOTO  900 

4000  PRINT  "STACKDEPLED  AT  STEP", Ml 

4O10  PRINT  "THE  LAST  ERROR  MAGNITUDE  WAS", Si 

4020  PRINT  "NEW  STACK  SIZE*****PREVIOUS  SlZE"jNl 

4030  PRINT  "OR  MAXIMAL  STACK  SIZE****IF  SO  RESPON  1 (ONE)" 

4040  INPUT  F8 

4050  IF  F8=l  THEN  4100 

4060  STOP 

4100  PRINT  "NEW  ERROR  TOLERANCE" 

4110  INPUT  E 

4120  PRINT  "NEW  STACK  S1ZE***’’,**PREV10US  SIZE",N1 
4130  INPUT  N1 
4140  GOTO  900 
9000  END 


STOP 
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